perm filename BEJCZY[AL,HE]1 blob
sn#283911 filedate 1977-05-27 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00011 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 BEJCZY - BEJCZY SIMPLIFIED DETERMINATION OF D MATRIX
C00005 00003 LINK INERTIAL VALUES FOR THE BLUE ARM
C00007 00004 PARAMETERS IN SIMPLIFIED STATE EQUATIONS FOR INERTIA AND GRAVITY LOADS.
C00009 00005 STORAGE AREA FOR BEJCZY COEFFICIENTS
C00011 00006 HNDWT & NOWT: MODIFY BEJCZY PARAMETERS FOR A GIVEN LOAD IN THE HAND.
C00013 00007 COMMON CODE FOR NOWT AND HNDWT
C00015 00008 DTERMS: COMPUTES THE D(II) AND D(I) COEFFICIENTS OF THE STATE EQUATIONS.
C00018 00009 "DTERMS" EXECUTABLE CODE
C00021 00010 [CONTINUATION OF DTERMS]
C00024 00011 [CONTINUATION OF DTERMS]
C00025 ENDMK
C⊗;
;BEJCZY - BEJCZY SIMPLIFIED DETERMINATION OF D MATRIX
TITLE BEJCZY
;THIS FILE CONTAINS THREE SAIL CALLABLE ROUTINES: HNDWT, NOWT, AND DTERMS.
;THEY CAN BE USED TO DETERMINE THE DIAGONAL TERMS OF THE ACCELERATION
;COEFFICIENTS MATRIX AS WELL AS THE GRAVITY MATRIX FOR THE STATE EQUATIONS
;DEFINING THE ARM DYNAMICS. THE IMPLEMENTED EQUATIONS WERE DERIVED IN
;"ROBOT ARM DYNAMICS AND CONTROL, JPL MEMORANDUM 33-669", BY A.K. BEJCZY.
;
;THE PURPOSE OF THE THREE ROUTINES AND THEIR REQUIRED ARGUMENTS ARE EXPLAINED
;IN THE FOLLOWING PAGES.
INTERNAL HNDWT,DTERMS,NOWT,G
EXTERNAL SNCOSD,SQRT
;DEFINITIONS
↓P ← 17 ;PUSH STACK POINTER
↓YELARM←← 1 ;MECHANISM BITS: YELLOW ARM, NOT INCLUDING HAND
↓YELHND←← 2 ;YELLOW HAND
↓BLUARM←← 4 ;BLUE ARM, NOT INCLUDING HAND
↓BLUHND←← 10 ;BLUE HAND
;LINK INERTIAL VALUES FOR THE BLUE ARM
;
; LINK MASS X Y Z I(XX) I(YY) I(ZZ)
; [OZ-MSEC↑2/IN] [INCHES] [OZ-IN-MSEC↑2]
;
; 1 513610.0 0 0 -1.69 5.494@6
; 2 259780.0 0 -4.28 -1.67 9.6329@6 2.0459@6 7.7522@6
; 3 208500.0 0 0 -20.4 7.9694@7 7.9694@7 39167.0
; 4 101390.0 0 0 1.323 633330.0 174440.0 505560.0
; 5 41408.0 0 0 2.82 440346.0 414080.0 38837.0
; 6 33643.0 0 0 -3.09 422775.0 395629.0 39335.0
;
;INPUT SHAFT INERTIAS IN [OZ-IN-MILLISECONDS**2]:
;
; LINK INERTIA
;
; 1 1.5000@8
; 2 4.0000@8
; 3 100000.0 (MODIFICATION OF ORIGINAL BEJCZY EQUATIONS)
; 4 1.0212@7
; 5 1.0212@7
; 6 848093.0
; 7 1.5900@6
;
;THE FORCE OF GRAVITY IS TAKEN TO BE:
;
; G = 3.8592*10@-4 OZ-MILLISECONDS**2/INCHES
;
;GEOMETRICAL PARAMETERS:
;
; R2=6.05
; R6=9.38 (YELLOW), R6=9.425 (BLUE)
;PARAMETERS IN SIMPLIFIED STATE EQUATIONS FOR INERTIA AND GRAVITY LOADS.
;ALL VALUES ARE FOR THE CASE OF NO LOAD BEING HELD IN THE HAND.
BB1 ←← 1.731E8 ;B1 = INPUT INERTIA 1 + I(1YY) + 2M(2)Y(2)R(2)
; + (M(2)+M(3)+M(4)+M(5))R(2)**2
; + .5( I(2XX) + I(2ZZ) )
BB2 ←← 7.969E7 ;B2 = I(3YY)
BB3 ←←-8.507E6 ;B3 = 2( M(3)Z(3) - M(4)Y(4) )
↓BB4 ←← 4.849E5 ;B4 = INPUT INERTIA 3 + M(3) + M(4) + M(5) + M(6)
BB5 ←← 6.598E5 ;B5 = 2M(5)Z(5) + 2M(6)*( Z(6) + R(6) )
BB13 ←← 4.817E8 ;B13 = INPUT INERTIA 2 + I(2YY) + I(3XX)
BB14 ←← 1.039E7 ;B14 = INPUT INERTIA 4 + I(4YY)
BB15 ←← 1.892E6 ;B15 = I(5XX) + I(6XX) + M(6)R(6)*( R(6) + 2Z(6) )
↓BB1718←← 0.9277E7
;↓BB1718←← 1.208E7 ;B17 + B18 = INPUT INERTIA 5 + I(5YY) + I(6XX)
; + M(6)R(6)* ( 2Z(6) + R(6) )
↓BB1920←← 8.874E5 ;B19 + B20 = INPUT INERTIA 6 + I(6ZZ)
DD1 ←←-4.687E6 ;D1 = M(2)Z(2) + M(3)Z(3) - M(4)Y(4)
GG ←← 3.859E-4 ;G = ACCELERATION DUE TO GRAVITY
NGG ←←-3.859E-4 ;-G
GDD2 ←← 1.486E2 ;G*D2= G*( BB4 - INPUT INERTIA 3 )
MGDD3 ←←-1.273E2 ;-G*D3=-G*( .5BB5 )
↓R6Z6 ←← 1.267E1 ;2*( R(6) + Z(6) )
↓R62Z6 ←← 3.058E1 ;R(6)*( R(6) + 2Z(6) )
BHDI ←← 0.000E0 ;BLUE HAND GRAVITY LOADING
BHDII ←← 1.590E6 ;BLUE HAND INERTIA
;STORAGE AREA FOR BEJCZY COEFFICIENTS
↓B1: BB1 ;UNCHANGED BY LOAD IN HAND
↓B2: BB2 ; " " " " "
↓B3: BB3 ; " " " " "
↓B13: BB13 ; " " " " "
↓B14: BB14 ; " " " " "
↓D1: DD1 ; " " " " "
↓G: GG ; " " " " "
↓NEGG: NGG ; " " " " "
↓B5: BB5 ;DEPENDS UPON LOAD IN HAND
↓B15: BB15 ; " " " " "
↓GD2: GDD2 ; " " " " "
↓MGD3: MGDD3 ; " " " " "
↓B4: BB4 ; " " " " "
;B1718 STORED IN DTERMS " " " " "
;B1920 " " " " " " " "
;LIST OF VALUES FOR LOAD DEPENDENT COEFFICENTS WITH NO LOAD IN HAND.
↓NO4: BB4
↓NO5: BB5
↓NO15: BB15
↓NO1718:BB1718
↓NO1920:BB1920
↓HOLD: BLOCK 17
;ARM HAND DYNAMIC COEFFICIENTS
↓BHDATA:BHDI
BHDII
↓YHDATA:BHDI
BHDII
;HNDWT & NOWT: MODIFY BEJCZY PARAMETERS FOR A GIVEN LOAD IN THE HAND.
;HNDWT IS USED TO ESTABLISH A LOAD BEING HELD IN THE HAND AND NOWT
;RESETS THE COEFFICIENTS FOR NO LOAD. SAMPLE SAIL CALLS TO THESE
;ROUTINES ARE AS FOLLOWS:
;
; NOWT(DUMMY ARGUMENT)
; OR
; HNDWT(ARRAY)
;
;WHERE ARRAY(1) CONTAINS THE WEIGHT OF THE LOAD IN OZ., ARRAY(2) THE
;R(XX) VALUE, ARRAY(3) THE R(YY) VALUE, AND ARRAY(4) THE R(ZZ) VALUE.
;ALL LENGTHS ARE IN INCHES. IT IS ASSUMED THAT THE CENTER OF MASS OF
;THE LOAD IS LOCATED AT THE ORIGIN OF THE HAND AXIS SYSTEM.
BEGIN HNDWT
MASS←←10 ;VALUE OF MASS
IX←←11 ;VALUE OF I(XX)
IZ←←12 ;VALUE OF I(ZZ)
AC←←13
↑NOWT: MOVEM 16,HOLD+16 ;SAVE ALL THE REGISTERS
HRRZI 16,HOLD
BLT 16,HOLD+15
SETZB MASS,IX ;INDICATE NO WEIGHT IN HAND
SETZ IZ
JRST SAME
↑HNDWT: MOVEM 16,HOLD+16 ;SAVE ALL THE REGISTERS
HRRZI 16,HOLD
BLT 16,HOLD+15
MOVE AC,-1(P) ;GET THE ADDR. OF THE VALUES
MOVE MASS,(AC) ;GET THE VALUE OF MASS IN OZ
FDVR MASS,G
MOVE IX,1(AC) ;GET THE VALUE OF R(XX)
FMPR IX,1(AC) ;COMPUTE I(XX)
FMPR IX,MASS
MOVE IZ,3(AC) ;GET THE VALUE OF R(ZZ)
FMPR IZ,3(AC) ;COMPUTE I(ZZ)
FMPR IZ,MASS
; COMMON CODE FOR NOWT AND HNDWT
SAME: MOVE AC,NO4 ;LOAD B4
FADR AC,MASS ;ADD THE MASS OF THE LOAD
MOVEM AC,B4 ;SAVE THE VALUE
PUSH P,AC ;COMPUTE THE SQUARE ROOT
PUSHJ P,SQRT
MOVEM 1,D33 ;STORE NEW VALUE IN DTERMS WHERE D33= SQRT B4
FMPR AC,G ;MULT. BY ACCEL. OF GRAVITY
MOVEM AC,GD2 ;STORE IN COEF. ARRAY
MOVE AC,[R6Z6] ;LOAD 2*( R(6) + Z(6) )
FMPR AC,MASS ;MULT. BY THE MASS
FADR AC,NO5 ;ADD NO LOAD VALUE OF B5
MOVEM AC,B5 ;STORE IN COEF. ARRAY
FMPR AC,[0.5] ;CALCULATE D3=.5*B5
FMPR AC,NEGG ;CALCULATE -G*D3
MOVEM AC,MGD3 ;STORE IN COEF. ARRAY
FMPR MASS,[R62Z6] ;MULT. MASS BY R(6)*( R(6) + 2Z(6) )
FADRB MASS,IX ;ADD THE I(XX) OF THE LOAD
FADR MASS,NO15 ;ADD THE NO LOAD VALUE OF B15
MOVEM MASS,B15 ;STORE IN COEF. ARRAY
FADR IX,NO1718 ;ADD TO IX NO LOAD B17+B18
PUSH P,IX ;COMPUTE THE SQUARE ROOT
PUSHJ P,SQRT
MOVEM 1,D55 ;STORE NEW VALUE IN DTERMS WHERE D55=B1718
FADR IZ,NO1920 ;ADD MASS I(ZZ) TO NO LOAD B19+B20
PUSH P,IZ ;COMPUTE THE SQUARE ROOT
PUSHJ P,SQRT
MOVEM 1,D66 ;STORE NEW VALUE IN DTERMS WHERE D66=B1920
HRLZI 16,HOLD ;RESTORE THE REGISTERS
BLT 16,16
SUB P,[2(2)] ;INDICATE VALUES OFF STACK
JRST @2(P) ;RETURN
BEND
;DTERMS: COMPUTES THE D(II) AND D(I) COEFFICIENTS OF THE STATE EQUATIONS.
;A SAMPLE SAIL PROCEDURE CALL TO THIS ROUTINE IS AS FOLLOWS:
;
; DTERMS(REAL ARRAY DD,TH;INTEGER MECHANISM);
;
;WHERE DD IS AN ARRAY IN WHICH THE SIX VALUES OF D(I) AND THE SIX VALUES
;OF D(II) ARE TO BE RETURNED. TH IS AN ARRAY WHICH CONTAINS THE JOINT
;VARIABLES FOR JOINTS 1 TO 6, AND MECHANISM IS A WORD WHICH INDICATES WHICH
;MECHANISM IS TO BE CONSIDERED. IT IS ASSUMED THAT THE TH VALUES ARE IN
;UNITS OF DEGREES OR INCHES.
BEGIN DTERMS
;REGISTER DEFINITIONS AND TEMPORARY STORAGE
SN←←1 ;SINE RESULTS OF SNCOSR RETURNED IN REG 1
CS←←2 ;COSINE " " " " " " 2
AC←←11
BC←←12
SN2←←13 ;SIN THETA 2
CS2←←14 ;COS THETA 2
SN5←←15 ;SIN THETA 5
CS5←←16 ;COS THETA 5
TH1: 0 ;STORAGE AREA FOR JOINT VARIABLES
TH2: 0
S3: 0
TH4: 0
TH5: 0
CD1: 0 ;GRAVITY LOADING TERM
D11: 0 ;DIAGONAL TERMS OF ACCELERATION MATRIX
CD2: 0
D22: 0
CD3: 0
↑D33: BB4 ;HAND LOAD DEPENDENT CONSTANT
CD4: 0
D44: 0
CD5: 0
↑D55: BB1718 ; " " " "
CD6: 0
↑D66: BB1920 ; " " " "
; "DTERMS" EXECUTABLE CODE
↑DTERMS:MOVEM 16,HOLD+16 ;SAVE THE REGISTERS
HRRZI 16,HOLD
BLT 16,HOLD+15
MOVE BC,-1(P) ;GET MECHANISM WORD
TRNE BC,YELHND+BLUHND ;CHECK IF JUST NEED VALUES FOR ARM HAND
JRST HANDS ;BRANCH IF JUST HAND DATA REQUESTED
HRL AC,-2(P) ;GET ADDR. OF TH ARRAY
HRRI AC,TH1 ;STORE STARTING AT TH1
BLT AC,TH5 ;GET THE FIRST 5 JOINT VARIABLES
;D33, D55, AND D66 ARE CONSTANTS DEPENDENT ONLY UPON HAND LOAD
;COMPUTE D44
PUSH P,TH5 ;CALCULATE SIN AND COS TH5
PUSHJ P,SNCOSD
MOVEM SN,SN5 ;SAVE THE VALUE
FMPR SN,SN5 ; (SIN TH5)**2
FMPR SN,B15 ; X B15
FADR SN,B14 ; + B14
MOVEM SN,D44 ;SAVE D44
;COMPUTE D22
MOVEM CS,CS5 ;ALREADY HAVE COS 5 IN REG 2, SAVE IT
FMPR CS,B5 ; X B5
FADR CS,B3 ; + B3
MOVE AC,S3 ;LOAD S3
FMPR AC,B4 ; X B4
FADR CS,AC ; + ( B3 + B5*CS5 )
FMPR CS,S3 ; X S3
MOVEM CS,BC ;SAVE RESULT IN BC TEMPORARILY
FADR CS,B13 ; + B13
MOVEM CS,D22 ;SAVE D22
FADR AC,D1 ;COMPUTE D1 + B4*S3 FOR LATER
MOVEM AC,CD2 ;SAVE IN CD2 TEMP.
;COMPUTE D11
PUSH P,TH2 ;GET VALUE OF SIN AND COS TH2
PUSHJ P,SNCOSD
MOVEM SN,SN2 ;SAVE THE VALUE
FMPR SN,SN2 ; (SIN TH2)**2
FADR BC,B2 ;ADD B2 TO ( B3 + B5*CS5 )*S3 + B4*S3**2
FMPR BC,SN ; X (SIN TH2)**2
FADR BC,B1 ; + B1
MOVEM BC,D11 ;SAVE D11
; [CONTINUATION OF DTERMS]
;COMPUTE CD3
MOVEM CS,CS2 ;SAVE THE VALUE OF COS TH2
FMPR CS,GD2 ; X (G * D2)
MOVEM CS,CD3 ;SAVE CD3
;COMPUTE CD5
PUSH P,TH4 ;GET VALUE OF SIN AND COS TH4
PUSHJ P,SNCOSD
MOVE AC,SN2 ;LOAD THE SIN TH2
FMPRB AC,CS5 ; X COS TH5, SAVE IN BOTH LOCATIONS
FMPR AC,SN ; X SIN TH4
FMPR CS2,SN5 ;MULT. COS TH2 BY SIN TH5
FADR AC,CS2 ;NOW HAVE SIN TH2*COS TH5*SIN TH4
; + COS TH2*SIN TH5
FMPR AC,MGD3 ; X (-G * D3)
MOVEM AC,CD5 ;SAVE CD5
;COMPUTE CD2
FMPR SN,CS2 ;NOW HAVE SIN TH4*COS TH2*SIN TH5
FADR SN,CS5 ; + SIN TH2*COS TH5
FMPR SN,MGD3 ; X (-G * D3)
MOVE AC,CD2 ;LOAD D1 + D2*S3
FMPR AC,NEGG ; X (-G)
FMPR AC,SN2 ; X SIN TH2
FADR AC,SN ; - G*D3*(SIN TH2*COS TH5
; +SIN TH4*COS TH2*SIN TH5)
MOVEM AC,CD2 ;SAVE CD2
;COMPUTE CD4
FMPR CS,SN5 ; COMPUTE COS TH4 X SIN TH5
FMPR CS,SN2 ; X SIN TH2
FMPR CS,MGD3 ; X (-G * D3)
MOVEM CS,CD4 ;SAVE CD4
;SEND BACK THE DII AND DI TERMS
MOVE AC,-3(P) ;GET THE ADDRESS OF THE DD ARRAY
MOVE BC,AC ;SAVE IT AGAIN
HRLI AC,CD1 ;START TRANSFER FROM FIRST GRAVITY TERM
BLT AC,13(BC) ;TRANSFER 12 TERMS FOR THE D[I] AND D[II]
JRST DTDNE ;EXIT
; [CONTINUATION OF DTERMS]
;SEND BACK DII AND DI FOR THE ARM HAND
HANDS: MOVE AC,-3(P) ;GET ADDR TO RETURN DATA TOO
HRLI AC,BHDATA ;ASSUME BLUE HAND DATA REQUESTED
TRNE BC,YELHND ;CHECK IF YELLOW HAND DATA REQ. INSTEAD
HRLI AC,YHDATA ;REPLACE IF SO
HRRZ BC,AC ;SAVE RETURN ADDR AGAIN
BLT AC,1(BC) ;TRANSFER 2 TERMS FOR THE D[I] AND D[II]
;RESTORE REGISTERS AND EXIT
DTDNE: HRLZI 16,HOLD ;RESTORE REGISTERS
BLT 16,16
SUB P,[4(4)] ;INDICATE ADDR. POPPED OFF STACK
JRST @4(P) ;RETURN
BEND
END